#include <gtest/gtest.h>
#include <gmock/gmock.h>

#define private public
#define protected public
#include <particle.h>
#undef private
#undef protected

using namespace std;
using ::testing::ContainerEq;


class ParticleTest: public::testing::Test
{
    protected:
        // Declares the particles
        Particle p1_ = Particle(1, 10, 10);          // 2D particle
        Particle p2_ = Particle(2, 20, 20, 20);      // 3D particle
        Particle p3_ = Particle(3, 30, 30, 30);      // 3D particle

        // other data
        StdArray6d zero_array_ = {0, 0, 0, 0, 0, 0};

        // You can remove any or all of the following functions if their bodies
        // would be empty.
        ParticleTest()
        {
            // You can do set-up work for each test here.
        };

        ~ParticleTest()
        {
            // You can do clean-up work that doesn't throw exceptions here.
        };

        // If the constructor and destructor are not enough for setting up
        // and cleaning up each test, you can define the following methods:
        void SetUp() override
        {
            // Code here will be called immediately after the constructor
            // (right before each test).
            cout << "Set Up Particle Test" << endl;
        };

        void TearDown() override
        {
            // Code here will be called immediately after each test (right
            // before the destructor).
        };

        // Class members declared here can be used by all tests in the test
        // suite for Foo.
};


// Tests that the Particle::Particle() method does Abc.
TEST_F(ParticleTest, IsEmptyInitiate)
{
    cout << "\n---------- ParticleTest IsEmptyInitiate ----------" << endl;
    cout << "check id... " << endl;
    EXPECT_EQ(p1_.id(), 1);
    EXPECT_EQ(p2_.id(), 2);
    EXPECT_EQ(p3_.id(), 3);

    cout << "check coordinate... " << endl;
    StdArray6d c1 = {10.0, 10.0, 0.0, 0.0, 0.0, 0.0};
    cout << "c1: ";
    for (int i = 0; i < 6; i++)
    {
        cout << c1[i] << ", ";
    }
    cout << endl;
    EXPECT_THAT(c1, ContainerEq(*(p1_.coordinate())));
    StdArray6d c2 = {20.0, 20.0, 20.0, 0.0, 0.0, 0.0};
    EXPECT_THAT(c2, ContainerEq(*(p2_.coordinate())));
    StdArray6d c3 = {30.0, 30.0, 30.0, 0.0, 0.0, 0.0};
    EXPECT_THAT(c3, ContainerEq(*(p3_.coordinate())));

    cout << "check mass... " << endl;
    Eigen::Matrix3d m, Im;
    m.setZero();
    Im.setZero();

    Mass mass = {.lumped_mass={0, 0, 0, 0, 0, 0},
              .translate_mass_matrix=m,
              .rotation_mass_matrix=Im};
    EXPECT_THAT(mass.lumped_mass, ContainerEq(p1_.mass()->lumped_mass));
    EXPECT_THAT(mass.lumped_mass, ContainerEq(p2_.mass()->lumped_mass));
    EXPECT_THAT(mass.lumped_mass, ContainerEq(p3_.mass()->lumped_mass));

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(m(i,j), p1_.mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p2_.mass()->translate_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(m(i,j), p3_.mass()->translate_mass_matrix(i,j));

            // rotation mass matrix
            EXPECT_DOUBLE_EQ(Im(i,j), p1_.mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p2_.mass()->rotation_mass_matrix(i,j));
            EXPECT_DOUBLE_EQ(Im(i,j), p3_.mass()->rotation_mass_matrix(i,j));
        }
    }

    cout << "check force... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.force())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p2_.force())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p3_.force())));

    cout << "check displace... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.displace())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p2_.displace())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p3_.displace())));

    cout << "check velocity... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.velocity())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p2_.velocity())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p3_.velocity())));

    cout << "check accelerate... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.accelerate())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p2_.accelerate())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p3_.accelerate())));

    cout << "check previous_displace... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_displace())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p2_.previous_displace())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p3_.previous_displace())));

    // cout << "check previous_velocity... " << endl;
    // EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_velocity())));
    // EXPECT_THAT(zero_array_, ContainerEq(*(p2_.previous_velocity())));
    // EXPECT_THAT(zero_array_, ContainerEq(*(p3_.previous_velocity())));

    // cout << "check previous_accelerate... " << endl;
    // EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_accelerate())));
    // EXPECT_THAT(zero_array_, ContainerEq(*(p2_.previous_accelerate())));
    // EXPECT_THAT(zero_array_, ContainerEq(*(p3_.previous_accelerate())));

    cout << "check next_displace... " << endl;
    // EXPECT_THAT(zero_array_, ContainerEq(p1_.next_displace_));
    // EXPECT_THAT(zero_array_, ContainerEq(p2_.next_displace_));
    // EXPECT_THAT(zero_array_, ContainerEq(p3_.next_displace_));

    cout << "check current_position... " << endl;
    EXPECT_THAT(c1, ContainerEq(*(p1_.current_position())));
    EXPECT_THAT(c2, ContainerEq(*(p2_.current_position())));
    EXPECT_THAT(c3, ContainerEq(*(p3_.current_position())));

    cout << "check previours_position... " << endl;
    const StdArray6d* pos3 = p3_.previous_position();
    cout << "previous postion: ";
    for (int i = 0; i < 6; i++)
    {
        cout << (*pos3)[i] << ", ";
    }
    cout << endl;
    EXPECT_THAT(c1, ContainerEq(*(p1_.previous_position())));
    EXPECT_THAT(c2, ContainerEq(*(p2_.previous_position())));
    EXPECT_THAT(c3, ContainerEq(*(p3_.previous_position())));

    cout << "check dof_index... " << endl;
    StdArray6b index = {false, false, false, false, false, false};
    EXPECT_THAT(index, ContainerEq(*(p1_.dof_index())));
    EXPECT_THAT(index, ContainerEq(*(p2_.dof_index())));
    EXPECT_THAT(index, ContainerEq(*(p3_.dof_index())));

    cout << "check dof_key... " << endl;
    std::set<std::string> key;
    EXPECT_THAT(key, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(key, ContainerEq(p2_.dof_key_));
    EXPECT_THAT(key, ContainerEq(p3_.dof_key_));

    cout << "check translate_mass_matrix_on... " << endl;
    EXPECT_FALSE(p1_.translate_mass_matrix_on_);
    EXPECT_FALSE(p2_.translate_mass_matrix_on_);
    EXPECT_FALSE(p3_.translate_mass_matrix_on_);

    cout << "check rotation_mass_matrix_on... " << endl;
    EXPECT_FALSE(p1_.rotation_mass_matrix_on_);
    EXPECT_FALSE(p2_.rotation_mass_matrix_on_);
    EXPECT_FALSE(p3_.rotation_mass_matrix_on_);
};


// Tests that the Particle::setCoordinate() method.
TEST_F(ParticleTest, setCoordinate)
{
    cout << "\n---------- ParticleTest setCoordinate ----------" << endl;
    StdArray6d val = {1, 2, 3, 4, 5, 6};
    p1_.setCoordinate(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.coordinate())));
};


// Tests that the Particle::activateDof(), deactivateDof() and constraintDof()
// method.
TEST_F(ParticleTest, OperateDOF)
{
    cout << "\n---------- ParticleTest activateDof ----------" << endl;
    std::set<std::string> keys;
    StdArray6b index = {false, false, false, false, false, false};
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Ux... " << endl;
    keys.insert("Ux");
    index[0] = true;
    p1_.activateDof("Ux");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));
    // do it again
    p1_.activateDof("Ux");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Uy... " << endl;
    keys.insert("Uy");
    index[1] = true;
    p1_.activateDof("Uy");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Uz... " << endl;
    keys.insert("Uz");
    index[2] = true;
    p1_.activateDof("Uz");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Rotx... " << endl;
    keys.insert("Rotx");
    index[3] = true;
    p1_.activateDof("Rotx");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Roty... " << endl;
    keys.insert("Roty");
    index[4] = true;
    p1_.activateDof("Roty");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "activate Rotz... " << endl;
    keys.insert("Rotz");
    index[5] = true;
    p1_.activateDof("Rotz");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "try wrong key ABC ..." << endl;
    p1_.activateDof("ABC");


    cout << "\n---------- ParticleTest constraintDof ----------" << endl;
    cout << "test constraint..." << endl;
    DOF d1 = {.key={true, false, true, false, true, true},
              .val={0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
    p1_.constraintDof(d1);
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.displace())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.velocity())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.accelerate())));
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_displace())));
    // EXPECT_THAT(zero_array_, ContainerEq(p1_.next_displace_));

    cout << "test constraint after setDisplce()..." << endl;
    const StdArray6d disp = {1, 2, 3, 4, 5, 6};
    p1_.setDisplace(disp);
    p1_.constraintDof(d1);
    StdArray6d target = {0.0, 2.0, 0.0, 4.0, 0.0, 0.0};
    EXPECT_THAT(target, ContainerEq(*(p1_.displace())));

    cout << "test constraint after deactivate Uy..." << endl;
    p1_.deactivateDof("Uy");
    p1_.setDisplace(disp);
    p1_.constraintDof(d1);
    EXPECT_THAT(target, ContainerEq(*(p1_.displace())));
    p1_.activateDof("Uy");

    cout << "test constraint after deactivate Ux...." << endl;
    p1_.deactivateDof("Ux");
    p1_.setDisplace(disp);
    p1_.constraintDof(d1);
    target[0] = 1.0;
    EXPECT_THAT(target, ContainerEq(*(p1_.displace())));
    p1_.activateDof("Ux");

    cout << "\n---------- ParticleTest deactivateDof ----------" << endl;
    cout << "deactivate Ux... " << endl;
    keys.erase("Ux");
    index[0] = false;
    p1_.deactivateDof("Ux");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));
    // do it again
    p1_.deactivateDof("Ux");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "deactivate Uy... " << endl;
    keys.erase("Uy");
    index[1] = false;
    p1_.deactivateDof("Uy");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "deactivate Uz... " << endl;
    keys.erase("Uz");
    index[2] = false;
    p1_.deactivateDof("Uz");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "deactivate Rotx... " << endl;
    keys.erase("Rotx");
    index[3] = false;
    p1_.deactivateDof("Rotx");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "deactivate Roty... " << endl;
    keys.erase("Roty");
    index[4] = false;
    p1_.deactivateDof("Roty");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "deactivate Rotz... " << endl;
    keys.erase("Rotz");
    index[5] = false;
    p1_.deactivateDof("Rotz");
    EXPECT_THAT(keys, ContainerEq(p1_.dof_key_));
    EXPECT_THAT(index, ContainerEq(p1_.dof_index_));

    cout << "try wrong key ABC ..." << endl;
    p1_.deactivateDof("ABC");
};


// Tests that the Particle::setLumpedMass() method does Abc.
TEST_F(ParticleTest, OperateMass)
{
    cout << "\n---------- ParticleTest OperateMass ----------" << endl;
    cout << "check setLumpedMass..." << endl;
    StdArray6d mass = {10, 20, 30, 40, 50, 60};
    p1_.setLumpedMass(mass);
    EXPECT_THAT(mass, ContainerEq(p1_.mass()->lumped_mass));
    mass[0] = -10;
    // negative mass, cerr
    p1_.activateDof("Ux");
    p1_.setLumpedMass(mass);

    cout << "check addLumpedMass..." << endl;
    mass[0] = 10;
    p1_.addLumpedMass(mass);
    const StdArray6d target = {0, 40, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(p1_.mass()->lumped_mass));

    cout << "check clearLumpedMass..." << endl;
    p1_.clearLumpedMass();
    EXPECT_THAT(zero_array_, ContainerEq(p1_.mass()->lumped_mass));


    cout << "check setTranslateMass..." << endl;
    Eigen::Matrix3d m;
    m << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    p1_.setTranslateMass(m);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(m(i,j), p1_.mass()->translate_mass_matrix(i,j));
        }
    }
    EXPECT_TRUE(p1_.translate_mass_matrix_on_);
    EXPECT_FALSE(p1_.rotation_mass_matrix_on_);

    cout << "check addTranslateMass..." << endl;
    p1_.addTranslateMass(m);
    Eigen::Matrix3d m2 = m + m;
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(m2(i,j), p1_.mass()->translate_mass_matrix(i,j));
        }
    }
    EXPECT_TRUE(p1_.translate_mass_matrix_on_);
    EXPECT_FALSE(p1_.rotation_mass_matrix_on_);

    cout << "check clearTranslateMass..." << endl;
    m.setZero();
    p1_.clearTranslateMass();
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(m(i,j), p1_.mass()->translate_mass_matrix(i,j));
        }
    }
    EXPECT_FALSE(p1_.translate_mass_matrix_on_);
    EXPECT_FALSE(p1_.rotation_mass_matrix_on_);

    cout << "check setInertiaMass..." << endl;
    Eigen::Matrix3d Im;
    Im << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    p1_.setInertiaMass(Im);
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(Im(i,j), p1_.mass()->rotation_mass_matrix(i,j));
        }
    }
    EXPECT_FALSE(p1_.translate_mass_matrix_on_);
    EXPECT_TRUE(p1_.rotation_mass_matrix_on_);

    cout << "check addInertiaMass..." << endl;
    p1_.addInertiaMass(Im);
    Eigen::Matrix3d Im2 = Im + Im;
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(Im2(i,j), p1_.mass()->rotation_mass_matrix(i,j));
        }
    }
    EXPECT_FALSE(p1_.translate_mass_matrix_on_);
    EXPECT_TRUE(p1_.rotation_mass_matrix_on_);

    cout << "check clearInertiaMass..." << endl;
    Im.setZero();
    p1_.clearInertiaMass();
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // translate mass matrix
            EXPECT_DOUBLE_EQ(Im(i,j), p1_.mass()->rotation_mass_matrix(i,j));
        }
    }
    EXPECT_FALSE(p1_.translate_mass_matrix_on_);
    EXPECT_FALSE(p1_.rotation_mass_matrix_on_);
};



// Tests that the Particle::setForce() method does Abc.
TEST_F(ParticleTest, OperateForce)
{
    cout << "\n---------- ParticleTest OperateForce ----------" << endl;
    cout << "check setForce... " << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setForce(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.force())));

    cout << "check addForce... " << endl;
    val[1] = -10;
    p1_.addForce(val);
    StdArray6d target = {20, 10, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(*(p1_.force())));

    cout << "check clearForce..." << endl;
    p1_.clearForce();
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.force())));
};



// Tests that the Particle::setDisplace() method does Abc.
TEST_F(ParticleTest, OperateDisplace)
{
    cout << "\n---------- ParticleTest OperateDisplace ----------" << endl;
    cout << "check setDisplace... " << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setDisplace(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.displace())));

    cout << "check addDisplace... " << endl;
    val[1] = -10;
    p1_.addDisplace(val);
    StdArray6d target = {20, 10, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(*(p1_.displace())));

    cout << "check clearDisplace..." << endl;
    p1_.clearDisplace();
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.displace())));
};


// Tests that the Particle::setVelocity() method does Abc.
TEST_F(ParticleTest, OperateVelocity)
{
    cout << "\n---------- ParticleTest OperateVelocity ----------" << endl;
    cout << "check setVelocity... " << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setVelocity(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.velocity())));

    cout << "check addVelocity... " << endl;
    val[1] = -10;
    p1_.addVelocity(val);
    StdArray6d target = {20, 10, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(*(p1_.velocity())));

    cout << "check clearVelocity..." << endl;
    p1_.clearVelocity();
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.velocity())));
};


// Tests that the Particle::setAccelerate() method does Abc.
TEST_F(ParticleTest, OperateAccelerate)
{
    cout << "\n---------- ParticleTest OperateAccelerate ----------" << endl;
    cout << "check setAccelerate... " << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setAccelerate(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.accelerate())));

    cout << "check addAccelerate... " << endl;
    val[1] = -10;
    p1_.addAccelerate(val);
    StdArray6d target = {20, 10, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(*(p1_.accelerate())));

    cout << "check clearAccelerate..." << endl;
    p1_.clearAccelerate();
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.accelerate())));
};


// Tests that the Particle::setPreviousDisplace() method does Abc.
TEST_F(ParticleTest, OperatePreviousDisplace)
{
    cout << "\n---------- ParticleTest OperatePreviousDisplace ----------" << endl;
    cout << "check setPreviousDisplace... " << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setPreviousDisplace(val);
    EXPECT_THAT(val, ContainerEq(*(p1_.previous_displace())));

    cout << "check addPreviousDisplace... " << endl;
    val[1] = -10;
    p1_.addPreviousDisplace(val);
    StdArray6d target = {20, 10, 60, 80, 100, 120};
    EXPECT_THAT(target, ContainerEq(*(p1_.previous_displace())));

    cout << "check clearPreviousDisplace..." << endl;
    p1_.clearPreviousDisplace();
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_displace())));
};


// // Tests that the Particle::setNextDisplace() method does Abc.
// TEST_F(ParticleTest, OperateNextDisplace)
// {
//     cout << "\n---------- ParticleTest OperateNextDisplace ----------" << endl;
//     cout << "check setNextDisplace... " << endl;
//     StdArray6d val = {10, 20, 30, 40, 50, 60};
//     p1_.setNextDisplace(val);
//     EXPECT_THAT(val, ContainerEq(*(p1_.next_displace())));

//     cout << "check addNextDisplace... " << endl;
//     val[1] = -10;
//     p1_.addNextDisplace(val);
//     StdArray6d target = {20, 10, 60, 80, 100, 120};
//     EXPECT_THAT(target, ContainerEq(*(p1_.next_displace())));

//     cout << "check clearNextDisplace..." << endl;
//     p1_.clearNextDisplace();
//     EXPECT_THAT(zero_array_, ContainerEq(*(p1_.next_displace())));
// };


// Tests that the Particle::updatePosition() method does Abc.
TEST_F(ParticleTest, updatePosition)
{
    cout << "\n---------- ParticleTest updatePosition ----------" << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setDisplace(val);
    val[0] = 0;
    p1_.setPreviousDisplace(val);
    p1_.updatePosition();

    StdArray6d target1 = {20, 30, 30, 40, 50, 60};
    StdArray6d target2 = {10, 30, 30, 40, 50, 60};
    EXPECT_THAT(target1, ContainerEq(*(p1_.current_position())));
    EXPECT_THAT(target2, ContainerEq(*(p1_.previous_position())));

    // try a little double number
    StdArray6d val1 = {0.01, 0, 0, 0, 0, 0};
    p1_.setDisplace(val1);
    p1_.updatePosition();
    target1 = {10.01, 10, 0, 0, 0, 0};
    EXPECT_THAT(target1, ContainerEq(*(p1_.current_position())));
};


// Tests that the Particle::clearAll() method does Abc.
TEST_F(ParticleTest, clearAll)
{
    cout << "\n---------- ParticleTest updatePosition ----------" << endl;
    StdArray6d val = {10, 20, 30, 40, 50, 60};
    p1_.setDisplace(val);
    val[0] = 0;
    p1_.setPreviousDisplace(val);
    p1_.updatePosition();

    p1_.clearAll();
    cout << "check force... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.force())));
    cout << "check displace... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.displace())));
    cout << "check velocity... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.velocity())));
    cout << "check accelerate... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.accelerate())));
    cout << "check previous_displace... " << endl;
    EXPECT_THAT(zero_array_, ContainerEq(*(p1_.previous_displace())));
    cout << "check next_displace... " << endl;
    // EXPECT_THAT(zero_array_, ContainerEq(p1_.next_displace_));
    StdArray6d target = {10, 10, 0, 0, 0, 0};
    cout << "check current_position... " << endl;
    EXPECT_THAT(target, ContainerEq(*(p1_.current_position())));
    cout << "check previous_position... " << endl;
    EXPECT_THAT(target, ContainerEq(*(p1_.previous_position())));
};


// Tests that the Particle::solveFirstStep() method does Abc.
TEST_F(ParticleTest, solveFirstStep)
{
    cout << "\n---------- ParticleTest solveFirstStep ----------" << endl;
    cout << "without InertiaMass..." << endl;
    double h = 0.001, zeta = 0.5;
    p1_.activateDof("Ux");
    p1_.activateDof("Uy");
    StdArray6d mass = {10, 10, 0, 0, 0, 0};
    p1_.setLumpedMass(mass);
    StdArray6d force = {10, 10, 0, 0, 0, 0};
    p1_.setForce(force);
    p1_.solveFirstStep(h, zeta);

    StdArray6d acc = {1, 1, 0, 0, 0, 0};
    double prev = 1.0 * h * h / 2.0 - h * 0 + 0;
    StdArray6d prevdisp = {prev, prev, 0, 0, 0, 0};
    double next = 1.0 * h * h / 2.0 + h * 0 + 0;
    StdArray6d nextdisp = {next, next, 0, 0, 0, 0};
    EXPECT_THAT(acc, ContainerEq(*(p1_.accelerate())));
    EXPECT_THAT(prevdisp, ContainerEq(*(p1_.previous_displace())));
    // EXPECT_THAT(nextdisp, ContainerEq(*(p1_.next_displace())));

    StdArray6d c1 = {10.0+prev, 10.0+prev, 0, 0, 0, 0};
    StdArray6d c2 = {10.0, 10.0, 0, 0, 0, 0};
    EXPECT_THAT(c1, ContainerEq(*(p1_.previous_position())));
    EXPECT_THAT(c2, ContainerEq(*(p1_.current_position())));

    cout << "with InertiaMass..." << endl;
    p1_.clearAll();
    p1_.activateDof("Rotx");
    p1_.activateDof("Roty");
    p1_.activateDof("Rotz");
    Eigen::Matrix3d Im;
    Im << 10, 1, 1,
          1, 10, 1,
          1, 1, 10;
    Eigen::Vector3d f, tmp;
    f << 10, 10, 10;
    tmp = Im.inverse() * f;

    p1_.setInertiaMass(Im);
    force = {0, 0, 0, 10, 10, 10};
    p1_.setForce(force);
    p1_.solveFirstStep(h, zeta);
    acc = {0, 0, 0, tmp(0), tmp(1), tmp(2)};
    cout << "tmp: " << tmp.transpose() << endl;
    prev = acc[3] * h * h / 2.0 - h * 0 + 0;
    cout << "prev: " << prev << endl;
    prevdisp = {0, 0, 0, prev, prev, prev};
    next = acc[3] * h * h / 2.0 + h * 0 + 0;
    nextdisp = {0, 0, 0, next, next, next};
    EXPECT_THAT(acc, ContainerEq(*(p1_.accelerate())));
    EXPECT_THAT(prevdisp, ContainerEq(*(p1_.previous_displace())));
    // EXPECT_THAT(nextdisp, ContainerEq(*(p1_.next_displace())));

    c1 = {10.0, 10.0, 0, prev, prev, prev};
    c2 = {10.0, 10.0, 0, 0, 0, 0};
    EXPECT_THAT(c1, ContainerEq(*(p1_.previous_position())));
    EXPECT_THAT(c2, ContainerEq(*(p1_.current_position())));
};


// Tests that the Particle::solve() method does Abc.
TEST_F(ParticleTest, solve)
{
/* hard to evaluate, some cases are writed
* test_solve.cpp for partcile without InertiaMass
*   (rotation_mass_matrix_on == false, translate_mass_matrix_on == false)
* test_solve_rot.cpp for particle with IntertiaMass
*   (rotation_mass_matrix_on == true, translate_mass_matrix_on == false)
*/
};